#include <stdint.h>
#include <stdbool.h>

#include <string.h>
#include <stdlib.h>
#include <math.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "rprintf.h"

#include "uart2.h"
#include "twi.h"
#include "nav.h"


#define SETBIT(reg,bit)      (reg |=  (1<<bit)) 
#define CLEARBIT(reg,bit)    (reg &=~ (1<<bit)) 

#define yon 	CLEARBIT(PORTB,4)
#define bon 	CLEARBIT(PORTG,3)
#define ron 	CLEARBIT(PORTG,4)
 
#define yoff 	SETBIT(PORTB,4)
#define boff	SETBIT(PORTG,3)
#define roff	SETBIT(PORTG,4)

// NMEA parsing variables
/*******************************/

unsigned char buffer[100]; //="$GPRMC,123519,A,5926.3260,N,1759.8398,E,37.5,90.0,090213,003.3,W,*77"; //Serial buffer to catch GPS data
char inBuffer=0;
char NMEAcomplete=0;
unsigned char counter=0; //general counter


// waypoint info
unsigned char wpts_loaded=0;


unsigned char chksum=0; //the checksum generated
unsigned char checksum_received=0; //Checksum received
unsigned char p=0;

unsigned char temp=0;
unsigned char tempchar=0;
int tempint=0;


char sendbuffer[6];


//GPS obtained information

unsigned char fix_position=0;//Valid gps position

double turn=0;




unsigned char recbyte;
unsigned int i=0;



long templong;
unsigned char* endptr;
static double deg=0;
static double minutesfrac;




int toInt(unsigned char hit, unsigned char lot) {
  int t=0;
  t=(hit<<8)+lot;
  return t;
}



unsigned char hi(int i) {
  return i >> 8;
}

unsigned char lo(int i) {
  return (char)i;
}



char sendint(unsigned char cmd,int data1, int data2) {
		sendbuffer[0]=cmd;
		sendbuffer[1]=hi(data1);
		sendbuffer[2]=lo(data1);
		sendbuffer[3]=hi(data2);
		sendbuffer[4]=lo(data2);
		
		chksum=sendbuffer[0]+sendbuffer[1]+sendbuffer[2]+sendbuffer[3]+sendbuffer[4];
		sendbuffer[5]=chksum;

		uartSendByte(1, sendbuffer[0]);
		uartSendByte(1, sendbuffer[1]);
		uartSendByte(1, sendbuffer[2]);
		uartSendByte(1, sendbuffer[3]);
		uartSendByte(1, sendbuffer[4]);
		uartSendByte(1, sendbuffer[5]);
	} // sendint

char sendlong(unsigned char cmd,long data) {
		sendbuffer[0]=cmd;
		sendbuffer[1]=data>>24;
		sendbuffer[2]=data>>16;
		sendbuffer[3]=data>>8;
		sendbuffer[4]=data;
		
		chksum=sendbuffer[0]+sendbuffer[1]+sendbuffer[2]+sendbuffer[3]+sendbuffer[4];
		sendbuffer[5]=chksum;

		uartSendByte(1, sendbuffer[0]);
		uartSendByte(1, sendbuffer[1]);
		uartSendByte(1, sendbuffer[2]);
		uartSendByte(1, sendbuffer[3]);
		uartSendByte(1, sendbuffer[4]);
		uartSendByte(1, sendbuffer[5]);
	} // sendint
  	  	

int main(void)
{

	yon;
	boff;
	roff;
 

	// LED pins as outputs
	SETBIT(DDRG,3);  // G6 as output
	SETBIT(DDRG,4);  // G4 as output
	SETBIT(DDRB,4);  // B4 as output

	// Add waypoints. Values in decimal degrees
	wpts_loaded=wp_add(59.43875,17.99899,100,50);
	wpts_loaded=wp_add(59.43884,18.01513,100,50);
	wpts_loaded=wp_add(59.43356,18.01408,100,50);
	wpts_loaded=wp_add(59.43457,18.000929,100,50);

	wp_next(2);
	wp_next(2);


	//	twiInit(100);

	   	
	// Uart initialization

	// Uart 0 handles GPS
	uart0Init(); 
	uartSetBaudRate(0,19200);

	// Uart 1 handles Base station comuunication
	uart1Init(); 
	uartSetBaudRate(1,19200);

	// Tell Base station AP is alive
	sendint(1,(int)wpts_loaded,0);
	sendlong(2,1234567890);

		

		
	  


	counter=0;
	inBuffer=0;
	//NMEAcomplete=1;
	NMEAcomplete=0;


	// forever loop
	for( ; ; )  {
	while (!uartReceiveBufferIsEmpty(1)) {
	    recbyte=uart1GetByte();
		if (inBuffer) {
	    	buffer[counter]=recbyte;
			// todo: check for buffer overflow
			counter++;
			if (counter>98)
				counter=0;
		  	if (recbyte==0x0A) {
		  		NMEAcomplete=1;
				inBuffer=0;
				bon;
			}
		}
		else {
			if (recbyte=='$') {
				counter=0;
		    	buffer[counter]=recbyte;
				// todo: check for buffer overflow
				counter++;
	    		inBuffer=1;
			}
		} 
	} // while
	if (NMEAcomplete) {
	  // parse nmea
	 	ron;
		
			
		// start parsing just after "GPGGA,"
		i = 7;
		// get time for last fix
	
		templong = strtol(&buffer[i],&endptr,10);

		while(buffer[i++] != ',');				// next field: valid fix
	
		// get valid flag "A" is valid data
		if (buffer[i]=='A') 
			last_pos.validdata=1;
		else last_pos.validdata=0;
		while(buffer[i++] != ',');				// next field: latitude
	
		// get latitude [ddmm.mmmmm]
	    deg = strtod(&buffer[i], &endptr);
		minutesfrac = modf(deg/100, &deg);
		last_pos.la = deg + (minutesfrac*100)/60;
	
		while(buffer[i++] != ',');				// next field: N/S indicator
	
		// correct latitute for N/S
		if(buffer[i] == 'S') last_pos.la = -last_pos.la;

		while(buffer[i++] != ',');				// next field: Longitude

		// get longitude [dddmm.mmmmm]
	    deg = strtod(&buffer[i], &endptr);
		minutesfrac = modf(deg/100, &deg);
		last_pos.lo = deg + (minutesfrac*100)/60;
	
		while(buffer[i++] != ',');				// next field: N/S indicator
	
		// correct latitute for E/W
		if(buffer[i] == 'S') last_pos.lo = -last_pos.lo;

		while(buffer[i++] != ',');				// next field: SOG

		// get SOG [nnn.n]
	    last_pos.sog = strtod(&buffer[i], &endptr);
	
		while(buffer[i++] != ',');				// next field: COG

		// get COG [nnn.n]
	    last_pos.cog = strtod(&buffer[i], &endptr);
	
	  	if (last_pos.validdata==1) {		
			turn=navigate();		 
		  // send turnangle to pc
			turn=turn*10;
			tempint=(int)turn;
			sendint(10,tempint, (int)getwpcourse());
		
			//Data sent for debugging reasons
			sendint(11,1,last_pos.validdata);
			sendlong(2,(long)(last_pos.lo*1000000));
			
			boff;
			roff;
		}
		NMEAcomplete=0;
		} //nmea complete
	
	}	// foreverloop
} // main

	






/*

	// Check for byte from PC
		if (!uartReceiveBufferIsEmpty(1)) {
			roff;
			recbyte=uart1GetByte();
		
		    if (recbyte==65) { 
				yoff;
				servoPos=1000;
				p=0x01;
				twiWrite(0x10,&p,1);
				p=0x00;
				twiWrite(0x10,&p,1);
				temp = servoPos>>8;
				twiWrite(0x10,&temp,1);
				temp = servoPos;
				twiWrite(0x10,&temp,1);
			
				bon;
			}
		
			if (recbyte==66) { 
				boff;
			
				servoPos=1000;
				p=0x01;
				twiWrite(0x10,&p,1);
				p=0x00;
				twiWrite(0x10,&p,1);
				temp = servoPos>>8;
				twiWrite(0x10,&temp,1);
				temp = servoPos;
				twiWrite(0x10,&temp,1);
			
				yon;
			}
			
		}  // if pc command byte
	  

*/
